#ifndef VISUAL_ODOMETRY_H
#define VISUAL_ODOMETRY_H
#include "myslam/common_include.h"
#include <opencv2/opencv.hpp>

#include "myslam/frame.h"
#include "myslam/map.h"

namespace myslam {

class VisualOdometry{

public:
    typedef shared_ptr<VisualOdometry> Ptr;
    enum VOState{
        INITIALIZING = -1,
        OK = 0,
        LOST
    };
    VOState  state_;
    Map::Ptr map_;
    Frame::Ptr ref_;
    Frame::Ptr curr_;
    cv::Ptr<cv::ORB> orb_;
    vector<cv::Point3f> pts_3d_ref_;
    vector<cv::KeyPoint> keypoints_curr_;
    Mat   descriptors_curr_;
    Mat   descriptors_ref_;
    vector<cv::DMatch> feature_matches_;
    SE3d T_c_r_estimated_;
    SE3d T_c_r_BA_;
    int num_inliers_;
    int num_lost_;
    //parameters
    int num_of_features_;
    double scale_factor_;
    int level_pyramid_;
    float match_ratio_;
    int max_num_lost_;
    int min_inliers_;
    double key_frame_min_rot;
    double Key_frame_min_trans;
public:
    VisualOdometry();
    ~VisualOdometry();
    bool addFrame(Frame::Ptr frame);
protected:
    void extractKeyPoints();
    void computeDescriptors();
    void featureMatching();
    void poseEstimationPnp();
    void setRef3DPoints();

    void addKeyFrame();
    bool checkEstimationPose();
    bool checkKeyFrame();
};

}




#endif // VISUAL_ODOMETRY_H
